{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "-cop8q3CTs-G"
   },
   "source": [
    "<center>\n",
    "<h4>CDS 110, Lecture 5</h4>\n",
    "<font color=blue><h1>State Estimation for a Kinematic Car Model</h1></font>\n",
    "<h3>Richard M. Murray, Winter 2024</h3>\n",
    "</center>\n",
    "\n",
    "[Open in Google Colab](https://colab.research.google.com/drive/1TESB0NzWS3XBxJa_hdOXMifICbBEDRz8)\n",
    "\n",
    "In this lecture, we will show how to construct an observer for a system in the presence of noise and disturbances.\n",
    "\n",
    "Recall that an observer is a system that takes as input the (noisy) measured output of a system along with the applied input to the system, and produces as estimate $\\hat x$ of the current state:\n",
    "\n",
    "<center>\n",
    "<img width=400 src=\"https://www.cds.caltech.edu/~murray/courses/cds110/sp2024/estimation.png\">\n",
    "</center>\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Import the various Python packages that we require\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from math import pi, sin, cos, tan\n",
    "try:\n",
    "  import control as ct\n",
    "  print(\"python-control\", ct.__version__)\n",
    "except ImportError:\n",
    "  !pip install control\n",
    "  import control as ct\n",
    "import control.flatsys as fs"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "c5UGnS73sH4c"
   },
   "source": [
    "## White noise\n",
    "\n",
    "A white noise process $W(t)$ is a signal that has the property that the mean of the signal is 0 and the value of the signal at any point in time $t$ is uncorrelated to the value of the signal at a point in time $s$, but that has a fixed amount of variance.  Mathematically, a white noise process $W\n",
    "(t) \\in \\mathbb{R}^k$ satisfies\n",
    "\n",
    "$$\n",
    "\\begin{aligned}\n",
    "\\mathbb{E}\\{W(t)\\} &= 0, &&\\text{for all $t$} \\\\\n",
    "\\mathbb{E}\\{W^\\mathtt{T}(t) W(s)\\} &= Q\\, \\delta(t-s) && \\text{for all $s, t$},\n",
    "\\end{aligned}\n",
    "$$\n",
    "\n",
    "where $Q \\in \\mathbb{R}^{k \\times k}$ is the \"intensity\" of the white noise process.\n",
    "\n",
    "The python-control function `white_noise` can be used to create an instantiation of a white noise process:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Create the time vector that we want to use\n",
    "Tf = 5\n",
    "T = np.linspace(0, Tf, 1000)\n",
    "dt = T[1] - T[0]\n",
    "\n",
    "# Create a white noise signal\n",
    "?ct.white_noise\n",
    "Q = np.array([[0.1]])\n",
    "W = ct.white_noise(T, Q)\n",
    "\n",
    "plt.figure(figsize=[5, 3])\n",
    "plt.plot(T, W[0])\n",
    "plt.xlabel('Time [s]')\n",
    "plt.ylabel('$V$');"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "MtAPkkCd14_g"
   },
   "source": [
    "To confirm this is a white noise signal, we can compute the correlation function\n",
    "\n",
    "$$\n",
    "\\rho(\\tau) = \\mathbb{E}\\{V^\\mathtt{T}(t) V(t + \\tau)\\} = Q\\, \\delta(\\tau),\n",
    "$$\n",
    "\n",
    "where $\\delta(\\tau)$ is the unit impulse function:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Correlation function for the input\n",
    "tau, r_W = ct.correlation(T, W)\n",
    "\n",
    "plt.plot(tau, r_W, 'r-')\n",
    "plt.xlabel(r'$\\tau$')\n",
    "plt.ylabel(r'$r_W(\\tau)$')\n",
    "\n",
    "# Compute out the area under the peak\n",
    "print(\"Signal covariance: \", Q.item())\n",
    "print(\"Area under impulse: \", np.max(W) * dt)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "1eN_MZ94tQ9v"
   },
   "source": [
    "## System definition: kinematic car\n",
    "\n",
    "We make use of a simple model for a vehicle navigating in the plane, known as the \"bicycle model\".  The kinematics of this vehicle can be written in terms of the contact point $(x, y)$ and the angle $\\theta$ of the vehicle with respect to the horizontal axis:\n",
    "\n",
    "<table>\n",
    "<tr>\n",
    "    <td width=\"50%\"><img src=\"https://fbswiki.org/wiki/images/5/52/Kincar.png\" width=480></td>\n",
    "    <td width=\"50%\">\n",
    "$$\n",
    "\\large\\begin{aligned}\n",
    "  \\dot x &= \\cos\\theta\\, v \\\\\n",
    "  \\dot y &= \\sin\\theta\\, v \\\\\n",
    "  \\dot\\theta &= \\frac{v}{l} \\tan \\delta\n",
    "\\end{aligned}\n",
    "$$\n",
    "    </td>\n",
    "</tr>\n",
    "</table>\n",
    "\n",
    "The input $v$ represents the velocity of the vehicle and the input $\\delta$ represents the turning rate. The parameter $l$ is the wheelbase."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# System definition\n",
    "# Function to compute the RHS of the system dynamics\n",
    "def kincar_update(t, x, u, params):\n",
    "    # Get the parameters for the model\n",
    "    l = params['wheelbase']             # vehicle wheelbase\n",
    "    deltamax = params['maxsteer']         # max steering angle (rad)\n",
    "\n",
    "    # Saturate the steering input\n",
    "    delta = np.clip(u[1], -deltamax, deltamax)\n",
    "\n",
    "    # Return the derivative of the state\n",
    "    return np.array([\n",
    "        np.cos(x[2]) * u[0],            # xdot = cos(theta) v\n",
    "        np.sin(x[2]) * u[0],            # ydot = sin(theta) v\n",
    "        (u[0] / l) * np.tan(delta)      # thdot = v/l tan(delta)\n",
    "    ])\n",
    "\n",
    "kincar_params={'wheelbase': 3, 'maxsteer': 0.5}\n",
    "\n",
    "# Create nonlinear input/output system\n",
    "kincar = ct.nlsys(\n",
    "    kincar_update, None, name=\"kincar\", params=kincar_params,\n",
    "    inputs=('v', 'delta'), outputs=('x', 'y', 'theta'),\n",
    "    states=('x', 'y', 'theta'))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Utility function to plot lane change manuever\n",
    "def plot_lanechange(t, y, u, figure=None, yf=None, label=None):\n",
    "    # Plot the xy trajectory\n",
    "    plt.subplot(3, 1, 1, label='xy')\n",
    "    plt.plot(y[0], y[1], label=label)\n",
    "    plt.xlabel(\"x [m]\")\n",
    "    plt.ylabel(\"y [m]\")\n",
    "    if yf is not None:\n",
    "        plt.plot(yf[0], yf[1], 'ro')\n",
    "\n",
    "    # Plot x and y as functions of time\n",
    "    plt.subplot(3, 2, 3, label='x')\n",
    "    plt.plot(t, y[0])\n",
    "    plt.ylabel(\"$x$ [m]\")\n",
    "\n",
    "    plt.subplot(3, 2, 4, label='y')\n",
    "    plt.plot(t, y[1])\n",
    "    plt.ylabel(\"$y$ [m]\")\n",
    "\n",
    "    # Plot the inputs as a function of time\n",
    "    plt.subplot(3, 2, 5, label='v')\n",
    "    plt.plot(t, u[0])\n",
    "    plt.xlabel(\"Time $t$ [sec]\")\n",
    "    plt.ylabel(\"$v$ [m/s]\")\n",
    "\n",
    "    plt.subplot(3, 2, 6, label='delta')\n",
    "    plt.plot(t, u[1])\n",
    "    plt.xlabel(\"Time $t$ [sec]\")\n",
    "    plt.ylabel(\"$\\\\delta$ [rad]\")\n",
    "\n",
    "    plt.subplot(3, 1, 1)\n",
    "    plt.title(\"Lane change manuever\")\n",
    "    if label:\n",
    "        plt.legend()\n",
    "    plt.tight_layout()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "5F-40uInyvQr"
   },
   "source": [
    "We next define a desired trajectory for the vehicle.  For simplicity, we use a piecewise linear trajectory and then stabilize the system around that trajectory.  We will learn in a later lecture how to do this is in more rigorous way.  For now, it is enough to know that this generates a feasible trajectory for the vehicle."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Generate a trajectory for the vehicle\n",
    "# Define the endpoints of the trajectory\n",
    "x0 = np.array([0., -4., 0.]); u0 = np.array([10., 0.])\n",
    "xf = np.array([40., 4., 0.]); uf = np.array([10., 0.])\n",
    "Tf = 4\n",
    "Ts = Tf / 100\n",
    "\n",
    "# First 0.6 seconds: drive straight\n",
    "T1 = np.linspace(0, 0.6, 15, endpoint=False)\n",
    "x1 = np.array([6, -4, 0])\n",
    "xd1 = np.array([x0 + (x1 - x0) * (t - T1[0]) / (T1[-1] - T1[0]) for t in T1]).transpose()\n",
    "\n",
    "# Next 2.8 seconds: change to the other lane\n",
    "T2 = np.linspace(0.6, 3.4, 70, endpoint=False)\n",
    "x2 = np.array([35, 4, 0])\n",
    "xd2 = np.array([x1 + (x2 - x1) * (t - T2[0]) / (T2[-1] - T2[0]) for t in T2]).transpose()\n",
    "\n",
    "# Final 0.6 seconds: drive straight\n",
    "T3 = np.linspace(3.4, Tf, 15, endpoint=False)\n",
    "xd3 = np.array([x2 + (xf - x2) * (t - T3[0]) / (T3[-1] - T3[0]) for t in T3]).transpose()\n",
    "\n",
    "T = np.hstack([T1, T2, T3])\n",
    "xr = np.hstack([xd1, xd2, xd3])\n",
    "ur = np.array([u0 for t in T]).transpose()\n",
    "\n",
    "# Now create a simple controller to stabilize the trajectory\n",
    "P = kincar.linearize(x0, u0)\n",
    "K, _, _ = ct.lqr(\n",
    "    kincar.linearize(x0, u0),\n",
    "    np.diag([10, 100, 1]), np.diag([10, 10])\n",
    ")\n",
    "\n",
    "# Construct a closed loop controller for the system\n",
    "ctrl, clsys = ct.create_statefbk_iosystem(kincar, K)\n",
    "resp = ct.input_output_response(clsys, T, [xr, ur], x0)\n",
    "\n",
    "xd = resp.states\n",
    "ud = resp.outputs[kincar.nstates:]\n",
    "\n",
    "plot_lanechange(T, xd, ud, label='feasible')\n",
    "plot_lanechange(T, xr, ur, label='reference')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Simulation of the open loop trajectory\n",
    "sys_resp = ct.input_output_response(kincar, T, ud, xd[:, 0])\n",
    "plt.plot(sys_resp.states[0], sys_resp.states[1])\n",
    "plt.axis([0, 40, -5, 5])\n",
    "plt.xlabel(\"$x$ [m]\")\n",
    "plt.ylabel(\"$y$ [m]\")\n",
    "plt.gca().set_aspect('equal')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "7V81jzfZtiRe"
   },
   "source": [
    "## State estimation\n",
    "\n",
    "To illustrate how we can estimate the state of the trajectory, we construct an observer that takes the measured inputs and outputs to the system and computes an estimate of the state, using a estimator with dynamics\n",
    "\n",
    "$$\n",
    "\\dot{\\hat x} = f(\\hat x, u) - L(C \\hat x - y)\n",
    "$$\n",
    "\n",
    "Note that we go ahead and use the nonlinear dynamics for the prediction term, but the linearization for the correction term.\n",
    "\n",
    "We can determine the estimator gain $L$ via multiple methods:\n",
    "* Eigenvalue placement\n",
    "* Optimal estimation (Kalman filter)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "Jt_5SUTBuN7-"
   },
   "source": [
    "### Eigenvalue placement"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Define the outputs to use for measurements\n",
    "C = np.eye(2, 3)\n",
    "\n",
    "# Compute the linearization of the nonlinear dynamics\n",
    "P = kincar.linearize([0, 0, 0], [10, 0])\n",
    "\n",
    "# Compute the gains via eigenvalue placement\n",
    "L = ct.place(P.A.T, C.T, [-1, -2, -3]).T\n",
    "\n",
    "# Estimator update law\n",
    "def estimator_update(t, xhat, u, params):\n",
    "    # Extract the inputs to the estimator\n",
    "    y = u[0:2]     # first two system outputs\n",
    "    u = u[2:4]     # inputs that were applied\n",
    "\n",
    "    # Update the state estimate\n",
    "    xhatdot = kincar.updfcn(t, xhat, u, kincar_params) \\\n",
    "      - params['L'] @ (C @ xhat - y)\n",
    "\n",
    "    # Return the derivative\n",
    "    return xhatdot\n",
    "\n",
    "estimator = ct.nlsys(\n",
    "    estimator_update, None, name='estimator',\n",
    "    states=kincar.nstates, params={'L': L},\n",
    "    inputs= kincar.state_labels[0:2] + kincar.input_labels,\n",
    "    outputs=[f'xh{i}' for i in range(kincar.nstates)],\n",
    ")\n",
    "print(estimator)\n",
    "print(estimator.params)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Run the estimator from a different initial condition\n",
    "estresp = ct.input_output_response(\n",
    "    estimator, T, [xd[0:2], ud], [0, -3, 0])\n",
    "\n",
    "fig, axs = plt.subplots(3, 1, figsize=[5, 4])\n",
    "\n",
    "axs[0].plot(estresp.time, estresp.outputs[0], 'b-', T, xd[0], 'r--')\n",
    "axs[0].set_ylabel(\"$x$\")\n",
    "axs[0].legend([r\"$\\hat x$\", \"$x$\"])\n",
    "\n",
    "axs[1].plot(estresp.time, estresp.outputs[1], 'b-', T, xd[1], 'r--')\n",
    "axs[1].set_ylabel(\"$y$\")\n",
    "\n",
    "axs[2].plot(estresp.time, estresp.outputs[2], 'b-', T, xd[2], 'r--')\n",
    "axs[2].set_ylabel(r\"$\\theta$\")\n",
    "axs[2].set_xlabel(\"Time $t$ [s]\")\n",
    "\n",
    "plt.tight_layout()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "KPkD-wSXt8d0"
   },
   "source": [
    "### Kalman filter\n",
    "\n",
    "An alternative mechanism for creating an estimator is through the use of optimal estimation (Kalman filtering).\n",
    "\n",
    "Suppose that we have (very) noisy measurements of the system position, and also have disturbances taht are applied to our control signal."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Disturbance and noise covariances\n",
    "Qv = np.diag([0.1**2, 0.01**2])\n",
    "Qw = np.eye(2) * 0.1**2\n",
    "\n",
    "u_noisy = ud + ct.white_noise(T, Qv)\n",
    "sys_resp = ct.input_output_response(kincar, T, u_noisy, xd[:, 0])\n",
    "\n",
    "# Create noisy version of the measurements\n",
    "y_noisy = sys_resp.outputs[0:2] + ct.white_noise(T, Qw)\n",
    "\n",
    "plt.plot(y_noisy[0], y_noisy[1], 'k-')\n",
    "plt.plot(sys_resp.outputs[0], sys_resp.outputs[1], 'b-')\n",
    "plt.axis([0, 40, -5, 5])\n",
    "plt.xlabel(\"$x$ [m]\")\n",
    "plt.ylabel(\"$y$ [m]\")\n",
    "plt.legend(['measured', 'actual'])\n",
    "plt.gca().set_aspect('equal')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A Kalman filter allows us to estimate the optimal state given measurements of the inputs and outputs, as well as knowledge of the covariance of the signals."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Compute the Kalman gains (linear quadratic estimator)\n",
    "L_kf, _, _ = ct.lqe(P.A, P.B, C, Qv, Qw)\n",
    "\n",
    "kfresp = ct.input_output_response(\n",
    "    estimator, T, [y_noisy, ud], [0, -3, 0],\n",
    "    params={'L': L_kf})\n",
    "\n",
    "fig, axs = plt.subplots(3, 1, figsize=[5, 4])\n",
    "\n",
    "axs[0].plot(T, y_noisy[0], 'k-')\n",
    "axs[0].plot(kfresp.time, kfresp.outputs[0], 'b-', T, sys_resp.outputs[0], 'r--')\n",
    "axs[0].set_ylabel(\"$x$\")\n",
    "axs[0].legend([r\"$\\hat x$\", \"$x$\"])\n",
    "\n",
    "axs[1].plot(T, y_noisy[1], 'k-')\n",
    "axs[1].plot(kfresp.time, kfresp.outputs[1], 'b-', T, sys_resp.outputs[1], 'r--')\n",
    "axs[1].set_ylabel(\"$y$\")\n",
    "\n",
    "axs[2].plot(kfresp.time, kfresp.outputs[2], 'b-', T, sys_resp.outputs[2], 'r--')\n",
    "axs[2].set_ylabel(r\"$\\theta$\")\n",
    "axs[2].set_xlabel(\"Time $t$ [s]\")\n",
    "\n",
    "plt.tight_layout()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "pMfHmzsW0Dqh"
   },
   "source": [
    "We can get a better view of the convergence by plotting the errors:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "fig, axs = plt.subplots(3, 1, figsize=[5, 4])\n",
    "\n",
    "axs[0].plot(kfresp.time, kfresp.outputs[0] - sys_resp.outputs[0])\n",
    "axs[0].plot([T[0], T[-1]], [0, 0], 'k--')\n",
    "axs[0].set_ylabel(\"$x$ error\")\n",
    "axs[0].set_ylim([-1, 1])\n",
    "\n",
    "axs[1].plot(kfresp.time, kfresp.outputs[1] - sys_resp.outputs[1])\n",
    "axs[1].plot([T[0], T[-1]], [0, 0], 'k--')\n",
    "axs[1].set_ylabel(\"$y$ error\")\n",
    "axs[1].set_ylim([-1, 1])\n",
    "\n",
    "axs[2].plot(kfresp.time, kfresp.outputs[2] - sys_resp.outputs[2])\n",
    "axs[2].plot([T[0], T[-1]], [0, 0], 'k--')\n",
    "axs[2].set_ylabel(r\"$\\theta$ error\")\n",
    "axs[2].set_xlabel(\"Time $t$ [s]\")\n",
    "axs[2].set_ylim([-0.2, 0.2])\n",
    "\n",
    "plt.tight_layout()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "nccW48C5tns9"
   },
   "source": [
    "## Output feedback control\n",
    "\n",
    "We next construct a controller that makes use of the estimated state.  We will attempt to control the longitudinal position using the steering angle as an input, with the velocity set to the desired velocity (no tracking of the longitudinal position)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Compute the linearization of the nonlinear dynamics\n",
    "P = kincar.linearize([0, 0, 0], [10, 0])\n",
    "\n",
    "# Extract out the linearized dynamics from delta to y\n",
    "Alat = P.A[1:3, 1:3]\n",
    "Blat = P.B[1:3, 1:2]\n",
    "Clat = P.C[1:2, 1:3]\n",
    "\n",
    "sys = ct.ss(Alat, Blat, Clat, 0)\n",
    "print(sys)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Construct a state space controller, using LQR\n",
    "Qx = np.diag([1, 10])\n",
    "Qu = np.diag([1])\n",
    "\n",
    "K, _, _ = ct.lqr(Alat, Blat, Qx, Qu)\n",
    "print(f\"{K=}\")\n",
    "\n",
    "kf = -1 / (Clat @ np.linalg.inv(Alat - Blat @ K) @ Blat)\n",
    "print(f\"{kf=}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "v5oHK9-XMrEv"
   },
   "source": [
    "### Direct state space feedback\n",
    "\n",
    "We start by checking the response of the system assuming that we measure the state directly.\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Construct a controller for the full system\n",
    "def ctrl_output(t, x, u, params):\n",
    "  r_v, r_y = u[0:2]\n",
    "  x = u[3:5]                # y, theta\n",
    "  return np.vstack([r_v, -K @ x + kf * r_y])\n",
    "ctrl = ct.nlsys(\n",
    "    None, ctrl_output, name='ctrl',\n",
    "    inputs=['r_v', 'r_y', 'x', 'y', 'theta'],\n",
    "    outputs=['v', 'delta']\n",
    ")\n",
    "print(ctrl)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Direct state feedback\n",
    "clsys_direct = ct.interconnect(\n",
    "    [kincar, ctrl],\n",
    "    inputs=['r_v', 'r_y'],\n",
    "    outputs=['x', 'y', 'theta', 'v', 'delta'],\n",
    ")\n",
    "print(clsys_direct)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Run a simulation\n",
    "clresp_direct = ct.input_output_response(\n",
    "    clsys_direct, T, [10, xd[1]], X0=[0, -3, 0])\n",
    "\n",
    "plt.plot(clresp_direct.outputs[0], clresp_direct.outputs[1])\n",
    "plt.plot(xd[0], xd[1], 'r--')\n",
    "# plt.plot(clresp.time, clresp.outputs[1])\n",
    "plt.xlabel(\"$x$ [m]\")\n",
    "plt.ylabel(\"$y$ [m]\")\n",
    "plt.gca().set_aspect('equal')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "J0iS9V8YT4Ox"
   },
   "source": [
    "Note the \"lag\" in the $x$ coordinate. This comes from the fact that we did not use feedback to maintain the longitudinal position as a function of time, compared with the desired trajectory.  To see this, we can look at the commanded speed ($v$) versus the desired speed:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plot_lanechange(T, xd, ud, label=\"desired\")\n",
    "plot_lanechange(T, clresp_direct.outputs[0:2], clresp_direct.outputs[-2:], label=\"actual\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "SDrkfC_LUPDu"
   },
   "source": [
    "From this plot we can also see that there is a very large input $\\delta$ applied at $t=0$.  This is something we would have to fix if we were to implement this on a physical system (-1 rad $\\approx -60^\\circ$!)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "KS0E2g6aMgC0"
   },
   "source": [
    "### Estimator-based control\n",
    "\n",
    "We now consider the case were we cannot directly measure the state, but instead have to estimate the state from the commanded input and measured output.  We can insert the estimator into the system model by reconnecting the inputs and outputs.  The `ct.interconnect` function provides the needed flexibility:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "?ct.interconnect"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "rgI9QjBMAy7b"
   },
   "source": [
    "We now create the system model that includes the estimator (observer).  Here is the system we are trying to construct:\n",
    "\n",
    "<img width=600 src=\"https://www.cds.caltech.edu/~murray/courses/cds110/sp2024/ssctrl.png\">\n",
    "</center>\n",
    "\n",
    "(Be careful with the notation: in the diagram above $y$ is the measured outputs, which for our system are the $x$ and $y$ position of the vehicle, so overusing the symbol $y$.)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Connect the system, estimator, and controller\n",
    "clsys_estim = ct.interconnect(\n",
    "    [kincar, estimator, ctrl],\n",
    "    inplist=['ctrl.r_v', 'ctrl.r_y', 'estimator.x', 'estimator.y'],\n",
    "    inputs=['r_v', 'r_y', 'noise_x', 'noise_y'],\n",
    "    outlist=[\n",
    "        'kincar.x', 'kincar.y', 'kincar.theta',\n",
    "        'estimator.xh0', 'estimator.xh1', 'estimator.xh2',\n",
    "        'ctrl.v', 'ctrl.delta'\n",
    "    ],\n",
    "    outputs=['x', 'y', 'theta', 'xhat', 'yhat', 'thhat', 'v', 'delta'],\n",
    "    connections=[\n",
    "        ['kincar.v', 'ctrl.v'],\n",
    "        ['kincar.delta', 'ctrl.delta'],\n",
    "        ['estimator.x', 'kincar.x'],\n",
    "        ['estimator.y', 'kincar.y'],\n",
    "        ['estimator.delta', 'ctrl.delta'],\n",
    "        ['estimator.v', 'ctrl.v'],\n",
    "        ['ctrl.x', 'estimator.xh0'],\n",
    "        ['ctrl.y', 'estimator.xh1'],\n",
    "        ['ctrl.theta', 'estimator.xh2'],\n",
    "    ],\n",
    ")\n",
    "print(clsys_estim)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Run a simulation with no noise first\n",
    "clresp_nonoise = ct.input_output_response(\n",
    "    clsys_estim, T, [10, xd[1], 0, 0], X0=[0, -3, 0, 0, -5, 0])\n",
    "\n",
    "plt.plot(clresp_nonoise.outputs[0], clresp_nonoise.outputs[1])\n",
    "plt.plot(xd[0], xd[1], 'r--')\n",
    "\n",
    "plt.xlabel(\"$x$ [m]\")\n",
    "plt.ylabel(\"$y$ [m]\")\n",
    "plt.gca().set_aspect('equal')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Add some noise\n",
    "Qv = np.diag([0.1**2, 0.01**2])\n",
    "Qw = np.eye(2) * 0.1**2\n",
    "\n",
    "u_noise = ct.white_noise(T, Qv)\n",
    "y_noise = ct.white_noise(T, Qw)\n",
    "\n",
    "# Run a simulation\n",
    "clresp_noisy = ct.input_output_response(\n",
    "    clsys_estim, T, [10, xd[1], y_noise], X0=[0, -3, 0, 0, -5, 0])\n",
    "\n",
    "plt.plot(clresp_direct.outputs[0], clresp_direct.outputs[1], label='direct')\n",
    "plt.plot(clresp_nonoise.outputs[0], clresp_nonoise.outputs[1], label='nonoise')\n",
    "plt.plot(clresp_noisy.outputs[0], clresp_noisy.outputs[1], label='noisy')\n",
    "plt.legend()\n",
    "plt.plot(xd[0], xd[1], 'r--')\n",
    "\n",
    "plt.xlabel(\"$x$ [m]\")\n",
    "plt.ylabel(\"$y$ [m]\")\n",
    "plt.gca().set_aspect('equal')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Plot the differences in y to make differences more clear\n",
    "plt.plot(\n",
    "    clresp_nonoise.time, clresp_nonoise.outputs[1] - clresp_direct.outputs[1],\n",
    "    label='nonoise')\n",
    "plt.plot(\n",
    "    clresp_noisy.time, clresp_noisy.outputs[1] - clresp_direct.outputs[1],\n",
    "    label='noisy')\n",
    "plt.legend()\n",
    "plt.plot([clresp_nonoise.time[0], clresp_nonoise.time[-1]], [0, 0], 'r--')\n",
    "\n",
    "plt.xlabel(\"Time [s]\")\n",
    "plt.ylabel(\"$y$ [m]\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Show the control inputs as well as the final trajectory\n",
    "plot_lanechange(T, xd, ud, label=\"desired\")\n",
    "plot_lanechange(T, clresp_noisy.outputs[0:2], clresp_noisy.outputs[-2:], label=\"actual\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "id": "ZfxhaU9p_W4w"
   },
   "source": [
    "### Things to try\n",
    "\n",
    "* Wrap a controller around the velocity (or $x$ position) in addition to the lateral ($y$) position\n",
    "* Change the amounts of noise in the sensor signal\n",
    "* Add disturbances to the dynamics (corresponding to wind, hills, etc)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
